/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/math/curve1d/piecewise_quintic_spiral_path.h"

#include "gtest/gtest.h"
#include "modules/planning/math/curve1d/quintic_spiral_path.h"

namespace apollo {
namespace planning {

TEST(PiecewiseQuinticSpiralPath, Evaluate) {
  double theta0 = 0.0;
  double kappa0 = 0.0;
  double dkappa0 = 0.0;

  double delta_s0 = 1.0;

  double theta1 = 0.1;
  double kappa1 = 1.0;
  double dkappa1 = 0.0;

  QuinticSpiralPath path0(theta0, kappa0, dkappa0, theta1, kappa1, dkappa1,
      delta_s0);

  double theta2 = 0.2;
  double kappa2 = 1.0;
  double dkappa2 = 0.0;

  double delta_s1 = 2.0;
  QuinticSpiralPath path1(theta1, kappa1, dkappa1, theta2, kappa2, dkappa2,
      delta_s1);

  PiecewiseQuinticSpiralPath piecewise_path(theta0, kappa0, dkappa0);
  piecewise_path.Append(theta1, kappa1, dkappa1, delta_s0);
  piecewise_path.Append(theta2, kappa2, dkappa2, delta_s1);

  EXPECT_NEAR(delta_s0 + delta_s1, piecewise_path.ParamLength(), 1.0e-8);
  EXPECT_NEAR(path0.Evaluate(0, 0.0), piecewise_path.Evaluate(0, 0.0), 1.0e-8);
  EXPECT_NEAR(path1.Evaluate(0, 0.0), piecewise_path.Evaluate(0, delta_s0),
      1.0e-8);
  EXPECT_NEAR(path1.Evaluate(0, path1.ParamLength()),
      piecewise_path.Evaluate(0, piecewise_path.ParamLength()), 1.0e-8);
}

}  // namespace planning
}  // namespace apollo
